<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Transitional//EN" "http://www.w3.org/TR/xhtml1/DTD/xhtml1-transitional.dtd">
<html xmlns="http://www.w3.org/1999/xhtml">
<head>
<meta http-equiv="Content-Type" content="text/xhtml;charset=UTF-8"/>
<title>OpenShoe: nav_eq.c File Reference</title>

<link href="tabs.css" rel="stylesheet" type="text/css"/>
<link href="doxygen.css" rel="stylesheet" type="text/css" />

<link href="search/search.css" rel="stylesheet" type="text/css"/>
<script type="text/javascript" src="jquery.js"></script>
<script type="text/javascript" src="search/search.js"></script>
<script type="text/javascript">
  $(document).ready(function() { searchBox.OnSelectItem(0); });
</script>

</head>
<body>
<div id="top"><!-- do not remove this div! -->


<div id="titlearea">
<table cellspacing="0" cellpadding="0">
 <tbody>
 <tr style="height: 56px;">
  
  
  <td style="padding-left: 0.5em;">
   <div id="projectname">OpenShoe
   &#160;<span id="projectnumber">1.0</span>
   </div>
   
  </td>
  
  
  
 </tr>
 </tbody>
</table>
</div>

<!-- Generated by Doxygen 1.7.5.1 -->
<script type="text/javascript">
var searchBox = new SearchBox("searchBox", "search",false,'Search');
</script>
  <div id="navrow1" class="tabs">
    <ul class="tablist">
      <li><a href="index.html"><span>Main&#160;Page</span></a></li>
      <li><a href="pages.html"><span>Related&#160;Pages</span></a></li>
      <li><a href="modules.html"><span>Modules</span></a></li>
      <li><a href="annotated.html"><span>Data&#160;Structures</span></a></li>
      <li class="current"><a href="files.html"><span>Files</span></a></li>
      <li>
        <div id="MSearchBox" class="MSearchBoxInactive">
        <span class="left">
          <img id="MSearchSelect" src="search/mag_sel.png"
               onmouseover="return searchBox.OnSearchSelectShow()"
               onmouseout="return searchBox.OnSearchSelectHide()"
               alt=""/>
          <input type="text" id="MSearchField" value="Search" accesskey="S"
               onfocus="searchBox.OnSearchFieldFocus(true)" 
               onblur="searchBox.OnSearchFieldFocus(false)" 
               onkeyup="searchBox.OnSearchFieldChange(event)"/>
          </span><span class="right">
            <a id="MSearchClose" href="javascript:searchBox.CloseResultsWindow()"><img id="MSearchCloseImg" border="0" src="search/close.png" alt=""/></a>
          </span>
        </div>
      </li>
    </ul>
  </div>
  <div id="navrow2" class="tabs2">
    <ul class="tablist">
      <li><a href="files.html"><span>File&#160;List</span></a></li>
      <li><a href="globals.html"><span>Globals</span></a></li>
    </ul>
  </div>
</div>
<div class="header">
  <div class="summary">
<a href="#func-members">Functions</a>  </div>
  <div class="headertitle">
<div class="title">nav_eq.c File Reference</div>  </div>
</div>
<div class="contents">

<p>The c-file for the OpenShoe navigation algorithm.  
<a href="#details">More...</a></p>
<div class="textblock"><code>#include &quot;<a class="el" href="nav__eq_8h_source.html">nav_eq.h</a>&quot;</code><br/>
</div><table class="memberdecls">
<tr><td colspan="2"><h2><a name="func-members"></a>
Functions</h2></td></tr>
<tr><td class="memItemLeft" align="right" valign="top"><a class="el" href="nav__types_8h.html#a37e1884b1f06826c49607cec459b4e8a">precision</a>&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="group__aux__func.html#gabf788491b9a7053e88a954b41f74b8f7">sqrt_hf</a> (<a class="el" href="nav__types_8h.html#a37e1884b1f06826c49607cec459b4e8a">precision</a> arg)</td></tr>
<tr><td class="mdescLeft">&#160;</td><td class="mdescRight">Function for calculating the square root.  <a href="group__aux__func.html#gabf788491b9a7053e88a954b41f74b8f7"></a><br/></td></tr>
<tr><td class="memItemLeft" align="right" valign="top"><a class="el" href="nav__types_8h.html#a37e1884b1f06826c49607cec459b4e8a">precision</a>&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="group__aux__func.html#ga54eaa66071c6606e7dbad9673e8418a1">vecnorm2</a> (<a class="el" href="nav__types_8h.html#a37e1884b1f06826c49607cec459b4e8a">precision</a> *arg_vec, uint8_t len)</td></tr>
<tr><td class="mdescLeft">&#160;</td><td class="mdescRight">Function that calculates the squared Euclidean norm of a vector.  <a href="group__aux__func.html#ga54eaa66071c6606e7dbad9673e8418a1"></a><br/></td></tr>
<tr><td class="memItemLeft" align="right" valign="top">void&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="group__aux__func.html#ga59b1148a3b2039b267dc5b740aa2196a">euler2rotation</a> (<a class="el" href="nav__types_8h.html#ab7675278cb555aa98b43c97694753329">mat3</a> rotmat, const <a class="el" href="nav__types_8h.html#a90c683614d896321009d3b3c401b764f">vec3</a> euler)</td></tr>
<tr><td class="mdescLeft">&#160;</td><td class="mdescRight">Function that converts Euler angles ([roll,pitch,yaw]) into a rotation matrix <img class="formulaInl" alt="$R_b^t$" src="form_25.png"/>.  <a href="group__aux__func.html#ga59b1148a3b2039b267dc5b740aa2196a"></a><br/></td></tr>
<tr><td class="memItemLeft" align="right" valign="top">void&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="group__aux__func.html#gaa74b4197980d69d90ff0bb2299df74e1">rotation2quat</a> (<a class="el" href="nav__types_8h.html#ad9a64f455fa02affaba6740746aae7b2">quat_vec</a> q, const <a class="el" href="nav__types_8h.html#ab7675278cb555aa98b43c97694753329">mat3</a> rotmat)</td></tr>
<tr><td class="mdescLeft">&#160;</td><td class="mdescRight">Function for converting a rotation matrix <img class="formulaInl" alt="$R_b^t$" src="form_25.png"/> to quaternions.  <a href="group__aux__func.html#gaa74b4197980d69d90ff0bb2299df74e1"></a><br/></td></tr>
<tr><td class="memItemLeft" align="right" valign="top">void&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="group__aux__func.html#ga9ac9f67ebac20a4953265127e3621f6c">quat2rotation</a> (<a class="el" href="nav__types_8h.html#ab7675278cb555aa98b43c97694753329">mat3</a> rotmat, const <a class="el" href="nav__types_8h.html#ad9a64f455fa02affaba6740746aae7b2">quat_vec</a> q)</td></tr>
<tr><td class="mdescLeft">&#160;</td><td class="mdescRight">Function for converting quaternions to a rotation matrix <img class="formulaInl" alt="$R_b^t$" src="form_25.png"/>.  <a href="group__aux__func.html#ga9ac9f67ebac20a4953265127e3621f6c"></a><br/></td></tr>
<tr><td class="memItemLeft" align="right" valign="top">void&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="group__aux__func.html#ga269e411a81080270d202618aece4f079">rotation2euler</a> (<a class="el" href="nav__types_8h.html#a90c683614d896321009d3b3c401b764f">vec3</a> euler, const <a class="el" href="nav__types_8h.html#ab7675278cb555aa98b43c97694753329">mat3</a> rotmat)</td></tr>
<tr><td class="mdescLeft">&#160;</td><td class="mdescRight">Function for converting a rotation matrix <img class="formulaInl" alt="$R_b^t$" src="form_25.png"/> to Euler angles ([roll,pitch,yaw]).  <a href="group__aux__func.html#ga269e411a81080270d202618aece4f079"></a><br/></td></tr>
<tr><td class="memItemLeft" align="right" valign="top">void&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="group__aux__func.html#ga5f9cb26cc3e73e922fac295a84401462">innovation_cov</a> (<a class="el" href="nav__types_8h.html#a0407e05d4f0216c13cca22570399e44d">mat3sym</a> re, <a class="el" href="nav__types_8h.html#a0f4089eb3ad75e0675d7f7d3914fddeb">mat9sym</a> pvec)</td></tr>
<tr><td class="mdescLeft">&#160;</td><td class="mdescRight">Function for calculating the Kalman filter innovation covariance.  <a href="group__aux__func.html#ga5f9cb26cc3e73e922fac295a84401462"></a><br/></td></tr>
<tr><td class="memItemLeft" align="right" valign="top">void&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="group__aux__func.html#ga06b4d19c89f6b3af0ca6c25d541d350a">invmat3sys</a> (<a class="el" href="nav__types_8h.html#a0407e05d4f0216c13cca22570399e44d">mat3sym</a> ainv, <a class="el" href="nav__types_8h.html#a0407e05d4f0216c13cca22570399e44d">mat3sym</a> a)</td></tr>
<tr><td class="mdescLeft">&#160;</td><td class="mdescRight">Function for inverting a 3 by 3 matrix hermitian matrix.  <a href="group__aux__func.html#ga06b4d19c89f6b3af0ca6c25d541d350a"></a><br/></td></tr>
<tr><td class="memItemLeft" align="right" valign="top">void&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="group__aux__func.html#gaa026397785026ed32818cf8008abffb3">max_value</a> (<a class="el" href="nav__types_8h.html#a37e1884b1f06826c49607cec459b4e8a">precision</a> *max_v, uint8_t *index, <a class="el" href="nav__types_8h.html#a37e1884b1f06826c49607cec459b4e8a">precision</a> *arg_vec)</td></tr>
<tr><td class="mdescLeft">&#160;</td><td class="mdescRight">Function that calculates the maximum value of a vector and returns the max value and the index of the vector element holding the maximum value.  <a href="group__aux__func.html#gaa026397785026ed32818cf8008abffb3"></a><br/></td></tr>
<tr><td class="memItemLeft" align="right" valign="top">void&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="group__miscel.html#ga6ee64d0b9a2c5e539a5dff1e04ff91c2">gravity</a> (void)</td></tr>
<tr><td class="mdescLeft">&#160;</td><td class="mdescRight">Function that calculates the magnitude of the local gravity vector based upon the WGS84 gravity model.  <a href="group__miscel.html#ga6ee64d0b9a2c5e539a5dff1e04ff91c2"></a><br/></td></tr>
<tr><td class="memItemLeft" align="right" valign="top">void&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="group__main__func.html#gaae1bbd924940610e1709300d40ca5a0f">update_imu_data_buffers</a> (void)</td></tr>
<tr><td class="mdescLeft">&#160;</td><td class="mdescRight">Function that updates the IMU data buffers with the latest values read from the IMU, and writes the IMU data to that should be process at the current iteration to the processing variables.  <a href="group__main__func.html#gaae1bbd924940610e1709300d40ca5a0f"></a><br/></td></tr>
<tr><td class="memItemLeft" align="right" valign="top">void&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="group__main__func.html#ga9169808fbcb9f517deb908960a69ca7a">strapdown_mechanisation_equations</a> (void)</td></tr>
<tr><td class="mdescLeft">&#160;</td><td class="mdescRight">Function for doing a time update of the mechanized navigation equations.  <a href="group__main__func.html#ga9169808fbcb9f517deb908960a69ca7a"></a><br/></td></tr>
<tr><td class="memItemLeft" align="right" valign="top">void&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="group__main__func.html#gac80ad2aa648edb47edb8f62883cb2a7d">time_up_data</a> (void)</td></tr>
<tr><td class="mdescLeft">&#160;</td><td class="mdescRight">Function for doing a time update of the Kalman filter state covariance.  <a href="group__main__func.html#gac80ad2aa648edb47edb8f62883cb2a7d"></a><br/></td></tr>
<tr><td class="memItemLeft" align="right" valign="top">void&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="group__main__func.html#ga75c4a936b2db01f930ba89babb822f69">gain_matrix</a> (void)</td></tr>
<tr><td class="mdescLeft">&#160;</td><td class="mdescRight">Function for calculating the Kalman filter gain matrix.  <a href="group__main__func.html#ga75c4a936b2db01f930ba89babb822f69"></a><br/></td></tr>
<tr><td class="memItemLeft" align="right" valign="top">void&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="group__main__func.html#gaa1673f6cf928e97c6372c56eddb4cc72">measurement_update</a> (void)</td></tr>
<tr><td class="mdescLeft">&#160;</td><td class="mdescRight">Function for doing a measurement update of the Kalman filter covariance.  <a href="group__main__func.html#gaa1673f6cf928e97c6372c56eddb4cc72"></a><br/></td></tr>
<tr><td class="memItemLeft" align="right" valign="top">void&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="group__main__func.html#ga8812381ada5c2786a10e378e4ca34377">correct_navigation_states</a> (void)</td></tr>
<tr><td class="mdescLeft">&#160;</td><td class="mdescRight">Function for correcting the navigation states given a zero-velocity detection.  <a href="group__main__func.html#ga8812381ada5c2786a10e378e4ca34377"></a><br/></td></tr>
<tr><td class="memItemLeft" align="right" valign="top">void&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="group__main__func.html#gaf0b7424f50d37755b6b0736808082299">ZUPT_detector</a> (void)</td></tr>
<tr><td class="mdescLeft">&#160;</td><td class="mdescRight">Function for detecting when the system has zero-velocity.  <a href="group__main__func.html#gaf0b7424f50d37755b6b0736808082299"></a><br/></td></tr>
<tr><td class="memItemLeft" align="right" valign="top">void&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="group__init.html#ga8b0f8dc8c2ae1ff3f5e4f27284cf8e77">initialize_navigation_algorithm</a> (void)</td></tr>
<tr><td class="mdescLeft">&#160;</td><td class="mdescRight">Function for initializing the navigation algorithm.  <a href="group__init.html#ga8b0f8dc8c2ae1ff3f5e4f27284cf8e77"></a><br/></td></tr>
<tr><td class="memItemLeft" align="right" valign="top">void&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="group__calib.html#ga272f4012a72ac416888be28bff0236e6">estimate_accelerometer_biases</a> (void)</td></tr>
<tr><td class="mdescLeft">&#160;</td><td class="mdescRight">Function that estimates the accelerometer biases given a matrix of the mean of the measured acceleration at different orientations.  <a href="group__calib.html#ga272f4012a72ac416888be28bff0236e6"></a><br/></td></tr>
<tr><td class="memItemLeft" align="right" valign="top">void&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="group__calib.html#gabb3f28a44a35898c77d789734914c4c2">calibrate_accelerometers</a> (void)</td></tr>
<tr><td class="mdescLeft">&#160;</td><td class="mdescRight">Function for calibrating the accelerometer biases.  <a href="group__calib.html#gabb3f28a44a35898c77d789734914c4c2"></a><br/></td></tr>
<tr><td class="memItemLeft" align="right" valign="top">void&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="group__miscel.html#ga0b4ee34435eebb426eb367120ee79d72">zupt_update</a> (void)</td></tr>
<tr><td class="mdescLeft">&#160;</td><td class="mdescRight">Routine collecting the functions which need to be run to make a ZUPT update.  <a href="group__miscel.html#ga0b4ee34435eebb426eb367120ee79d72"></a><br/></td></tr>
<tr><td colspan="2"><h2><a name="var-members"></a>
Variables</h2></td></tr>
<tr><td colspan="2"><div class="groupHeader">Accelerometer calibration parameters.</div></td></tr>
<tr><td colspan="2"><div class="groupText"><p>Parameters controlling accelerometer calibration, and vectors and matrices used store the biases. </p>
</div></td></tr>
<tr><td class="memItemLeft" align="right" valign="top"><a class="el" href="nav__types_8h.html#a90c683614d896321009d3b3c401b764f">vec3</a>&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="group__nav__eq.html#ga1089eea940f41eff26a6cba1172089e9">accelerometer_biases</a></td></tr>
<tr><td class="mdescLeft">&#160;</td><td class="mdescRight">Accelerometer biases (x,y,z-axis) [ <img class="formulaInl" alt="$m/s^2$" src="form_17.png"/>].  <a href="group__nav__eq.html#ga1089eea940f41eff26a6cba1172089e9"></a><br/></td></tr>
<tr><td class="memItemLeft" align="right" valign="top"><a class="el" href="nav__types_8h.html#a37e1884b1f06826c49607cec459b4e8a">precision</a>&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="group__nav__eq.html#gad5049be4d6ed3b9f92063c6ca6413b46">acceleration_variance_threshold</a> = 0.002</td></tr>
<tr><td class="mdescLeft">&#160;</td><td class="mdescRight">Threshold used to check that accelerometers were stationary during the calibration [ <img class="formulaInl" alt="$(m/s^2)^2$" src="form_18.png"/>].  <a href="group__nav__eq.html#gad5049be4d6ed3b9f92063c6ca6413b46"></a><br/></td></tr>
<tr><td class="memItemLeft" align="right" valign="top">uint32_t&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="group__nav__eq.html#gab9d05ce5927a225127eff6993856da19">nr_of_calibration_samples</a> = 800</td></tr>
<tr><td class="mdescLeft">&#160;</td><td class="mdescRight">Number of samples used at each orientation in the calibration procedure.  <a href="group__nav__eq.html#gab9d05ce5927a225127eff6993856da19"></a><br/></td></tr>
<tr><td class="memItemLeft" align="right" valign="top">uint8_t&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="group__nav__eq.html#ga642a3ad33a0242f4e12fe46a13b0303f">nr_of_calibration_orientations</a> = 6</td></tr>
<tr><td class="mdescLeft">&#160;</td><td class="mdescRight">Number of orientations used in the accelerometer calibration procedure. OBS! Most be at least 3 and less than 13.  <a href="group__nav__eq.html#ga642a3ad33a0242f4e12fe46a13b0303f"></a><br/></td></tr>
<tr><td class="memItemLeft" align="right" valign="top">Bool&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="group__nav__eq.html#ga9340627920ea2445fe1469547bc64eae">new_orientation_flag</a> = false</td></tr>
<tr><td class="mdescLeft">&#160;</td><td class="mdescRight">Flag that is set to true when the IMU should be place in a new orientation. Should be set to false when the calibration procedure is started, and when the IMU has been placed in a new orientation by the user.  <a href="group__nav__eq.html#ga9340627920ea2445fe1469547bc64eae"></a><br/></td></tr>
<tr><td class="memItemLeft" align="right" valign="top">Bool&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="group__nav__eq.html#gaa3b35b0b909a9a4b40d82658fab54349">acc_calibration_finished_flag</a> = false</td></tr>
<tr><td class="mdescLeft">&#160;</td><td class="mdescRight">Flag that is set to true when the calibration is finished. Must be set to false before the calibration is started.  <a href="group__nav__eq.html#gaa3b35b0b909a9a4b40d82658fab54349"></a><br/></td></tr>
<tr><td colspan="2"><div class="groupHeader">General control parameters.</div></td></tr>
<tr><td colspan="2"><div class="groupText"><p>Parameters controlling general settings of the navigation algorithm </p>
</div></td></tr>
<tr><td class="memItemLeft" align="right" valign="top"><a class="el" href="nav__types_8h.html#a37e1884b1f06826c49607cec459b4e8a">precision</a>&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="group__nav__eq.html#ga98eaf96c6c207db4b40273a599149ebe">latitude</a> = 13</td></tr>
<tr><td class="mdescLeft">&#160;</td><td class="mdescRight">Rough latitude of the system [ <img class="formulaInl" alt="$degrees$" src="form_19.png"/>]. (Used to calculate the magnitude of the gravity vector)  <a href="group__nav__eq.html#ga98eaf96c6c207db4b40273a599149ebe"></a><br/></td></tr>
<tr><td class="memItemLeft" align="right" valign="top"><a class="el" href="nav__types_8h.html#a37e1884b1f06826c49607cec459b4e8a">precision</a>&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="group__nav__eq.html#ga0387cded24d9bdfa34c78dc27bef01e8">altitude</a> = 920</td></tr>
<tr><td class="mdescLeft">&#160;</td><td class="mdescRight">Rough altitude of the system [ <img class="formulaInl" alt="$m$" src="form_20.png"/>]. (Used to calculate the magnitude of the gravity vector)  <a href="group__nav__eq.html#ga0387cded24d9bdfa34c78dc27bef01e8"></a><br/></td></tr>
<tr><td class="memItemLeft" align="right" valign="top"><a class="el" href="nav__types_8h.html#a37e1884b1f06826c49607cec459b4e8a">precision</a>&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="group__nav__eq.html#gac7d75e5db7af21a75e50ad6437827947">g</a> = 9.782940329221166</td></tr>
<tr><td class="mdescLeft">&#160;</td><td class="mdescRight">Magnitude of the local gravity acceleration [ <img class="formulaInl" alt="$m/s^2$" src="form_17.png"/>].  <a href="group__nav__eq.html#gac7d75e5db7af21a75e50ad6437827947"></a><br/></td></tr>
<tr><td class="memItemLeft" align="right" valign="top"><a class="el" href="nav__types_8h.html#a37e1884b1f06826c49607cec459b4e8a">precision</a>&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="group__nav__eq.html#gae7e97f885bb0d70931c5c70ad200880d">dt</a> = 0.001220703125000</td></tr>
<tr><td class="mdescLeft">&#160;</td><td class="mdescRight">Sampling period [ <img class="formulaInl" alt="$s$" src="form_21.png"/>].  <a href="group__nav__eq.html#gae7e97f885bb0d70931c5c70ad200880d"></a><br/></td></tr>
<tr><td class="memItemLeft" align="right" valign="top">uint8_t&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="group__nav__eq.html#ga3f996476795917a4b1ace8c6c5e4b28a">error_signal</a></td></tr>
<tr><td class="mdescLeft">&#160;</td><td class="mdescRight">Error signaling vector. If zero no error has occurred.  <a href="group__nav__eq.html#ga3f996476795917a4b1ace8c6c5e4b28a"></a><br/></td></tr>
<tr><td colspan="2"><div class="groupHeader">IMU data buffer variables.</div></td></tr>
<tr><td colspan="2"><div class="groupText"><p>Vectors and variables related to the IMU data buffer. </p>
</div></td></tr>
<tr><td class="memItemLeft" align="right" valign="top"><a class="el" href="nav__types_8h.html#a90c683614d896321009d3b3c401b764f">vec3</a>&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="group__nav__eq.html#gaf92cab44915b7d9faaf5aeadb328bae6">accelerations_in</a></td></tr>
<tr><td class="mdescLeft">&#160;</td><td class="mdescRight">Accelerations read from the IMU [ <img class="formulaInl" alt="$m/s^2$" src="form_17.png"/>]. These are written into the IMU data buffer.  <a href="group__nav__eq.html#gaf92cab44915b7d9faaf5aeadb328bae6"></a><br/></td></tr>
<tr><td class="memItemLeft" align="right" valign="top"><a class="el" href="nav__types_8h.html#a90c683614d896321009d3b3c401b764f">vec3</a>&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="group__nav__eq.html#ga8cac4b2812dcb492989d7122c5eb0bfb">angular_rates_in</a></td></tr>
<tr><td class="mdescLeft">&#160;</td><td class="mdescRight">Angular rates read from the IMU [ <img class="formulaInl" alt="$rad/s$" src="form_22.png"/>]. These are written into the IMU data buffer.  <a href="group__nav__eq.html#ga8cac4b2812dcb492989d7122c5eb0bfb"></a><br/></td></tr>
<tr><td class="memItemLeft" align="right" valign="top"><a class="el" href="nav__types_8h.html#a90c683614d896321009d3b3c401b764f">vec3</a>&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="group__nav__eq.html#ga0e1577a397995052e34d61ddbf578956">accelerations_out</a></td></tr>
<tr><td class="mdescLeft">&#160;</td><td class="mdescRight">Accelerations outputted from the IMU data buffer [ <img class="formulaInl" alt="$m/s^2$" src="form_17.png"/>].  <a href="group__nav__eq.html#ga0e1577a397995052e34d61ddbf578956"></a><br/></td></tr>
<tr><td class="memItemLeft" align="right" valign="top"><a class="el" href="nav__types_8h.html#a90c683614d896321009d3b3c401b764f">vec3</a>&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="group__nav__eq.html#gabdb2113f7c6caefe1b25826c1f7c8324">angular_rates_out</a></td></tr>
<tr><td class="mdescLeft">&#160;</td><td class="mdescRight">Angular rates outputted from the IMU data buffer [ <img class="formulaInl" alt="$rad/s$" src="form_22.png"/>].  <a href="group__nav__eq.html#gabdb2113f7c6caefe1b25826c1f7c8324"></a><br/></td></tr>
<tr><td colspan="2"><div class="groupHeader">Initialization control parameters</div></td></tr>
<tr><td colspan="2"><div class="groupText"><p>Parameters controlling the initialization of the navigation algorithm, i.e., the initial states of the inertial navigation system equations and the initial Kalman filter covariance matrix. </p>
</div></td></tr>
<tr><td class="memItemLeft" align="right" valign="top">Bool&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="group__nav__eq.html#gac64a954fe1bf13df62e1e96920a36035">initialize_flag</a> = true</td></tr>
<tr><td class="mdescLeft">&#160;</td><td class="mdescRight">A flag that should be set to true when initialization is started and that becomes false when the initialization is finished.  <a href="group__nav__eq.html#gac64a954fe1bf13df62e1e96920a36035"></a><br/></td></tr>
<tr><td class="memItemLeft" align="right" valign="top">uint8_t&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="group__nav__eq.html#gaf3c89a8b1365434986dc0314c1c541cf">nr_of_inital_alignment_samples</a> = 16</td></tr>
<tr><td class="mdescLeft">&#160;</td><td class="mdescRight">Number of samples used in the initial alignment.  <a href="group__nav__eq.html#gaf3c89a8b1365434986dc0314c1c541cf"></a><br/></td></tr>
<tr><td class="memItemLeft" align="right" valign="top"><a class="el" href="nav__types_8h.html#a37e1884b1f06826c49607cec459b4e8a">precision</a>&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="group__nav__eq.html#gaf18deb1376fcc761f2ec2d947cf004e7">initial_heading</a> = 0</td></tr>
<tr><td class="mdescLeft">&#160;</td><td class="mdescRight">Initial heading [ <img class="formulaInl" alt="$rad$" src="form_23.png"/>].  <a href="group__nav__eq.html#gaf18deb1376fcc761f2ec2d947cf004e7"></a><br/></td></tr>
<tr><td class="memItemLeft" align="right" valign="top"><a class="el" href="nav__types_8h.html#a90c683614d896321009d3b3c401b764f">vec3</a>&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="group__nav__eq.html#ga7cd4f9c9472377e6ef5935727ada7109">initial_pos</a> = {0, 0, 0}</td></tr>
<tr><td class="mdescLeft">&#160;</td><td class="mdescRight">Initial position (North, East, Down) [ <img class="formulaInl" alt="$m$" src="form_20.png"/>].  <a href="group__nav__eq.html#ga7cd4f9c9472377e6ef5935727ada7109"></a><br/></td></tr>
<tr><td class="memItemLeft" align="right" valign="top"><a class="el" href="nav__types_8h.html#a90c683614d896321009d3b3c401b764f">vec3</a>&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="group__nav__eq.html#ga0ace968269836475a5c59c16280d1bf1">sigma_initial_position</a> = {0.00001,0.00001,0.00001}</td></tr>
<tr><td class="mdescLeft">&#160;</td><td class="mdescRight">Standard deviations in the initial position uncertainties [ <img class="formulaInl" alt="$m$" src="form_20.png"/>].  <a href="group__nav__eq.html#ga0ace968269836475a5c59c16280d1bf1"></a><br/></td></tr>
<tr><td class="memItemLeft" align="right" valign="top"><a class="el" href="nav__types_8h.html#a90c683614d896321009d3b3c401b764f">vec3</a>&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="group__nav__eq.html#ga5abe69320e5702cb7f4b79abb9bcf297">sigma_initial_velocity</a> = {0.01,0.01,0.01}</td></tr>
<tr><td class="mdescLeft">&#160;</td><td class="mdescRight">Standard deviations in the initial velocity uncertainties [ <img class="formulaInl" alt="$m/s$" src="form_24.png"/>].  <a href="group__nav__eq.html#ga5abe69320e5702cb7f4b79abb9bcf297"></a><br/></td></tr>
<tr><td class="memItemLeft" align="right" valign="top"><a class="el" href="nav__types_8h.html#a90c683614d896321009d3b3c401b764f">vec3</a>&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="group__nav__eq.html#gaad9757cf97cfd938818e7785a3beafe1">sigma_initial_attitude</a> = {0.00174,0.00174,0.00174}</td></tr>
<tr><td class="mdescLeft">&#160;</td><td class="mdescRight">Standard deviations in the initial attitude uncertainties [ <img class="formulaInl" alt="$rad$" src="form_23.png"/>].  <a href="group__nav__eq.html#gaad9757cf97cfd938818e7785a3beafe1"></a><br/></td></tr>
<tr><td colspan="2"><div class="groupHeader">Kalman filter control parameters</div></td></tr>
<tr><td colspan="2"><div class="groupText"><p>Parameters controlling the behavior of the Kalman filter. The parameters can be changed while the filter is running to adapt the filter to the current motion dynamics.</p>
<dl class="note"><dt><b>Note:</b></dt><dd>The default noise standard deviation figures are not set to reflect the true noise figures of the IMU sensors, but rather to model the sum of all the errors (biases, scale factors, nonlinearities, etc.) in the system and the measurement model. </dd></dl>
</div></td></tr>
<tr><td class="memItemLeft" align="right" valign="top"><a class="el" href="nav__types_8h.html#a37e1884b1f06826c49607cec459b4e8a">precision</a>&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="group__nav__eq.html#ga5add0877d1169e9e1d4dd227c1bcc83d">sigma_acceleration</a> = 0.7</td></tr>
<tr><td class="mdescLeft">&#160;</td><td class="mdescRight">Accelerometer process noise standard deviation [ <img class="formulaInl" alt="$m/s^2$" src="form_17.png"/>].  <a href="group__nav__eq.html#ga5add0877d1169e9e1d4dd227c1bcc83d"></a><br/></td></tr>
<tr><td class="memItemLeft" align="right" valign="top"><a class="el" href="nav__types_8h.html#a37e1884b1f06826c49607cec459b4e8a">precision</a>&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="group__nav__eq.html#gab6e7cd8a13de230fbcef8ad4e8001eb6">sigma_gyroscope</a> = 0.005235987755983</td></tr>
<tr><td class="mdescLeft">&#160;</td><td class="mdescRight">Gyroscope process noise standard deviation [ <img class="formulaInl" alt="$rad/s$" src="form_22.png"/>].  <a href="group__nav__eq.html#gab6e7cd8a13de230fbcef8ad4e8001eb6"></a><br/></td></tr>
<tr><td class="memItemLeft" align="right" valign="top"><a class="el" href="nav__types_8h.html#a90c683614d896321009d3b3c401b764f">vec3</a>&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="group__nav__eq.html#gae79b582d3d0132b84117242e1f6103f3">sigma_velocity</a> = {0.1,0.1,0.1}</td></tr>
<tr><td class="mdescLeft">&#160;</td><td class="mdescRight">Pseudo zero-velocity measurement noise standard deviations (north, east, down) [ <img class="formulaInl" alt="$m/s$" src="form_24.png"/>].  <a href="group__nav__eq.html#gae79b582d3d0132b84117242e1f6103f3"></a><br/></td></tr>
<tr><td colspan="2"><div class="groupHeader">Navigation and filter state variables.</div></td></tr>
<tr><td colspan="2"><div class="groupText"><p>Vectors that holds the current navigation state estimate and the covariance and gain of the Kalman filter. </p>
</div></td></tr>
<tr><td class="memItemLeft" align="right" valign="top"><a class="el" href="nav__types_8h.html#a90c683614d896321009d3b3c401b764f">vec3</a>&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="group__nav__eq.html#ga2807b9fb4b79e01dfde022b613d2575f">position</a></td></tr>
<tr><td class="mdescLeft">&#160;</td><td class="mdescRight">Position estimate (North,East,Down) [ <img class="formulaInl" alt="$m$" src="form_20.png"/>].  <a href="group__nav__eq.html#ga2807b9fb4b79e01dfde022b613d2575f"></a><br/></td></tr>
<tr><td class="memItemLeft" align="right" valign="top"><a class="el" href="nav__types_8h.html#a90c683614d896321009d3b3c401b764f">vec3</a>&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="group__nav__eq.html#ga589efe00d8bd4e8a69613f98390a04c7">velocity</a></td></tr>
<tr><td class="mdescLeft">&#160;</td><td class="mdescRight">Velocity estimate (North,East,Down) [ <img class="formulaInl" alt="$m/s$" src="form_24.png"/>].  <a href="group__nav__eq.html#ga589efe00d8bd4e8a69613f98390a04c7"></a><br/></td></tr>
<tr><td class="memItemLeft" align="right" valign="top"><a class="el" href="nav__types_8h.html#ad9a64f455fa02affaba6740746aae7b2">quat_vec</a>&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="group__nav__eq.html#ga059b83d370e7e6a5e9728a8f91943051">quaternions</a></td></tr>
<tr><td class="mdescLeft">&#160;</td><td class="mdescRight">Attitude (quaternions) estimate.  <a href="group__nav__eq.html#ga059b83d370e7e6a5e9728a8f91943051"></a><br/></td></tr>
<tr><td class="memItemLeft" align="right" valign="top"><a class="el" href="nav__types_8h.html#ab7675278cb555aa98b43c97694753329">mat3</a>&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="group__nav__eq.html#ga42753cbc9ae76483da3cbc331a011586">Rb2t</a></td></tr>
<tr><td class="mdescLeft">&#160;</td><td class="mdescRight">Rotation matrix used as an "aiding" variable in the filter algorithm. Holds the same information as the quaternions.  <a href="group__nav__eq.html#ga42753cbc9ae76483da3cbc331a011586"></a><br/></td></tr>
<tr><td class="memItemLeft" align="right" valign="top"><a class="el" href="nav__types_8h.html#a0f4089eb3ad75e0675d7f7d3914fddeb">mat9sym</a>&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="group__nav__eq.html#ga74c09cdfc5b901e7f842dbe42ed8f4f4">cov_vector</a></td></tr>
<tr><td class="mdescLeft">&#160;</td><td class="mdescRight">Vector representation of the Kalman filter covariance matrix.  <a href="group__nav__eq.html#ga74c09cdfc5b901e7f842dbe42ed8f4f4"></a><br/></td></tr>
<tr><td class="memItemLeft" align="right" valign="top"><a class="el" href="nav__types_8h.html#af1d09a15760e21f5de26f0839f1b6c62">mat9by3</a>&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="group__nav__eq.html#gac1df051c6f380aa8d1598b15ee9e6eca">kalman_gain</a></td></tr>
<tr><td class="mdescLeft">&#160;</td><td class="mdescRight">Vector representation of the Kalman filter gain matrix.  <a href="group__nav__eq.html#gac1df051c6f380aa8d1598b15ee9e6eca"></a><br/></td></tr>
<tr><td colspan="2"><div class="groupHeader">Zero-velocity detector control parameters</div></td></tr>
<tr><td colspan="2"><div class="groupText"><p>Parameters controlling the behavior of the zero-velocity detector. All the detector control parameters, except the <em>detector_Window_size</em> may be changed will the navigation algorithm is running in order to adapt the behavior of the detector to the current motion dynamics. </p>
</div></td></tr>
<tr><td class="memItemLeft" align="right" valign="top"><a class="el" href="nav__types_8h.html#a37e1884b1f06826c49607cec459b4e8a">precision</a>&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="group__nav__eq.html#ga519e1c68258a371960249e09c24e9943">sigma_acc_det</a> = 0.035</td></tr>
<tr><td class="mdescLeft">&#160;</td><td class="mdescRight">Accelerometer noise standard deviation figure [ <img class="formulaInl" alt="$m/s^2$" src="form_17.png"/>], which is used to control how much the detector should trusts the accelerometer data.  <a href="group__nav__eq.html#ga519e1c68258a371960249e09c24e9943"></a><br/></td></tr>
<tr><td class="memItemLeft" align="right" valign="top"><a class="el" href="nav__types_8h.html#a37e1884b1f06826c49607cec459b4e8a">precision</a>&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="group__nav__eq.html#ga779a972b079accc690d64bcd7bef3c91">sigma_gyro_det</a> = 0.006</td></tr>
<tr><td class="mdescLeft">&#160;</td><td class="mdescRight">Gyroscope noise standard deviation figure [ <img class="formulaInl" alt="$rad/s$" src="form_22.png"/>], which is used to control how much the detector should trusts the gyroscope data.  <a href="group__nav__eq.html#ga779a972b079accc690d64bcd7bef3c91"></a><br/></td></tr>
<tr><td class="memItemLeft" align="right" valign="top">volatile uint8_t&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="group__nav__eq.html#ga9e9b826ff87a8e0e4400661c27988682">detector_Window_size</a> = 3</td></tr>
<tr><td class="mdescLeft">&#160;</td><td class="mdescRight">The data window size used in the detector (OBS! Must be an odd number.).  <a href="group__nav__eq.html#ga9e9b826ff87a8e0e4400661c27988682"></a><br/></td></tr>
<tr><td class="memItemLeft" align="right" valign="top"><a class="el" href="nav__types_8h.html#a37e1884b1f06826c49607cec459b4e8a">precision</a>&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="group__nav__eq.html#ga34d28cf12521d9706da4ddfca54106bf">detector_threshold</a> = 50000</td></tr>
<tr><td class="mdescLeft">&#160;</td><td class="mdescRight">Threshold used in the detector.  <a href="group__nav__eq.html#ga34d28cf12521d9706da4ddfca54106bf"></a><br/></td></tr>
<tr><td class="memItemLeft" align="right" valign="top">bool&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="group__nav__eq.html#gabd51dc1d49060099533807a22acc61d4">zupt</a> = false</td></tr>
<tr><td class="mdescLeft">&#160;</td><td class="mdescRight">Flag that is set to true if a zero-velocity update should be done.  <a href="group__nav__eq.html#gabd51dc1d49060099533807a22acc61d4"></a><br/></td></tr>
<tr><td class="memItemLeft" align="right" valign="top"><a class="el" href="nav__types_8h.html#a37e1884b1f06826c49607cec459b4e8a">precision</a>&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="group__nav__eq.html#gae420c23e5d1fdf5b157535212ddeba9e">Test_statistics</a> = 0</td></tr>
<tr><td class="mdescLeft">&#160;</td><td class="mdescRight">Variable holding the test statistics for the generalized likelihood ratio test, i.e., the zero-velocity detector.  <a href="group__nav__eq.html#gae420c23e5d1fdf5b157535212ddeba9e"></a><br/></td></tr>
</table>
<hr/><a name="details" id="details"></a><h2>Detailed Description</h2>
<div class="textblock"><p>The c-file for the OpenShoe navigation algorithm. </p>
<p>This is the c-file for the OpenShoe navigation algorithm. It includes the signal processing functions needed to implement a zero-velocity aided inertial navigation system using a nine state Kalman filter working in a complimentary feedback configuration. It also includes the functions needed for implementing a accelerometer bias calibration framework.</p>
<dl class="author"><dt><b>Authors:</b></dt><dd>John-Olof Nilsson, Isaac Skog </dd></dl>
<dl class="copyright"><dt><b>Copyright:</b></dt><dd>Copyright (c) 2011 OpenShoe, ISC License (open source) </dd></dl>
</div></div>
<!-- window showing the filter options -->
<div id="MSearchSelectWindow"
     onmouseover="return searchBox.OnSearchSelectShow()"
     onmouseout="return searchBox.OnSearchSelectHide()"
     onkeydown="return searchBox.OnSearchSelectKey(event)">
<a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(0)"><span class="SelectionMark">&#160;</span>All</a><a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(1)"><span class="SelectionMark">&#160;</span>Data Structures</a><a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(2)"><span class="SelectionMark">&#160;</span>Files</a><a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(3)"><span class="SelectionMark">&#160;</span>Functions</a><a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(4)"><span class="SelectionMark">&#160;</span>Variables</a><a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(5)"><span class="SelectionMark">&#160;</span>Typedefs</a><a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(6)"><span class="SelectionMark">&#160;</span>Defines</a></div>

<!-- iframe showing the search results (closed by default) -->
<div id="MSearchResultsWindow">
<iframe src="javascript:void(0)" frameborder="0" 
        name="MSearchResults" id="MSearchResults">
</iframe>
</div>



<hr class="footer"/><address class="footer"><small>
Generated on Tue Dec 20 2011 12:12:36 for OpenShoe by &#160;<a href="http://www.doxygen.org/index.html">
<img class="footer" src="doxygen.png" alt="doxygen"/>
</a> 1.7.5.1
</small></address>

</body>
</html>
